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Abstract — In this paper, we have described the coordinate (position) estimation of automatic steered car by 
using kalman filter and prior knowledge of position of car i.e. its state equation. The kalman filter is one of the 
most widely used method for tracking and estimation due to its simplicity, optimality, tractability and 
robustness. However, the application to non linear system is difficult but in extended kalman filter we make it 
easy as we first linearize the system so that kalman filter can be applied. Kalman has been designed to integrate 
map matching and GPS system which is used in automatic vehicle location system and very useful tool in 
navigation. It takes errors or uncertainties via covariance matrix and then implemented to nullify those 
uncertainties. This paper reviews the motivation, development, use, and implications of the Kalman Filter. 
Index Terms — Kalman Filter(KF),Extended Kalman Filter(EKF), Unscented Kalman Filter(UKF). 



I. Introduction 

The main objective of any on road Automatic Vehicle system is that It should be navigated with route 
information properly to the benefit of the user by relating vehicle location to its surrounding .To be effective, the 
vehicle's position must be continuously maintained in real-time as position of automatic system changes with the 
time. 

Currently there are many techniques available for positioning of automatic vehicle location system, such as 
(odometer and compass technique). But these techniques are not reliable (dead reckon) techniques [1],[2] and 
[3] because they require updates to overcome the disturbance which comes into the picture with time and 
distance. In odometer [4], fixed objects in the surrounding provide landmarks which are listed in a data base. 
The system calculates the angle to each landmark and then orients the camera. A Kalman Filter is used to 
correct the position and orientation of the vehicle from the error between the observed and estimated angle to 
each landmark. Also to overcome the limitation of GPS technique it will be used combined with map matching 
but it also has drawback with congested road and overcrowded building area. In this paper we are highlighting 
the significance of kalman Filter in automatic vehicle location system. 

Rudolf E. Kalman gave the idea of kalman filter, the great use Kalman filter is due to its small computational 
requirement, recursive properties, and its status as the optimal estimator for non linear and linear systems with 
Gaussian error statistics [5]. Typical uses of the Kalman filter include smoothing noisy data and providing 
estimates of parameters of interest. Applications include global positioning system receivers, motors, robot and 
many more. 

A. Process Model Of The System 

The Kalman filter model assumes that the state of a system at a time k evolved from the prior state at time k-1 
according to the equation 

X(k) = A (k) x (£-1) + B (t) U (k) + w (k) (1) 
Where 

> X(k) is the state vector containing the terms of interest for the system 
(e.g., position, velocity, heading) at Time k. 

P U (k) is the vector containing any Control inputs (steering angle, throttle Setting, braking force). 

P A (k) is the state transition matrix which applies the effect of each system state parameter at time k-l on the 
system state at time k (e.g., the position and velocity at time k-l both affect the position at time k). 
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> B (k) is the control input matrix which applies the effect of each, control input parameter in the vector u (k) 
on the state vector (e.g., applies the effect of the throttle setting on the system velocity and position). 

> W (k) is the vector containing the process noise terms for each parameter in the state vector. The process 
noise is assumed to be drawn from a zero mean multivariate normal distribution with covariance given by 
the covariance matrix Q (k). 

B. Measurement Model Of The System 

Y (k) = C (k) x(k) + y (k) (2) 



Where 

P Y (k) is the vector of measurements 

> C (k) is the transformation matrix that maps the state vector parameters into the measurement domain. 

> V (k) is the vector containing the measurement noise terms for each 

observation in the measurement vector. Like the process noise, the measurement noise is assumed to be zero 
mean Gaussian white noise with covariance R (k). 

II.Kaman Filter Algorithm 

Kaman filter complete its operation in three stages. 

lit predicts the system evolution by using prior knowledge in (k-1). 

2. It measures the output in time (k). 

3. It update measured output in time (k). 

Kalman filter assumes that all predicted posterior density point at ever time step is Gaussian function and hence 
it is parameterized by mean and covariance. 

A. Propagation stage 

X(k+l)/ (k) =A(k) X (k) (3) 

P (k+ 1 )/ (k) = A P (k)/(k) A T + Q (4) 
And 

[(jo(k)(x)(k)T] = Q 

B. Measurement Stage 

> Y(k) = C(k)x(k) + v(k) (10) 

C. Correction and Update Stage 

K(k+l)=P(k+l)/(k)C T [CP(k+l)(k) C T ]+R _1 (H) 

> P (k+l)/(k+l) =(I-K(k+l)C)P(k+l)/(k) (12) 

X(k+l)/(k+l) =[X(k+l)/(k) +K(k+l)(Y(k)-C X(fc+l)/(/c))] (13) 

So by introducing the term kalman gain K we can update our predicted equation i.e. whatever will be the error 
in these equation can be compensated by selecting computing the kalman gain from measurement. 

D. Recursive Kalman Gain 
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. 1 



Compute Kalman Gain 

K, = P l{l _ l Ht[H,P,,,_ l H!' +R,] 



— 



Update error covahancc 
P, = (1-K,H,)P^_, 

P,_ n , = A,P,.4,' + G,Qtf 



On-Line or Off-Liue 

This does not depend on measurements 



Initial Slate 
Estimate .v n 



Measurement v, 



Update State Estimate with 
new measurement 

*, = -t-,_, + A*,(y, - H,x^ A ) 



State Estimate .v, 



Real-Time 
Ou-Liue 



III. Types Of Kalman Filter 

A. Extended Kalman Filter (EKF) 

As we know no system in practical world is linear, In extended kalman filter, we need to first linearize the 
system by taking the jacobian of state transition matrix(A) and measurement matrix (C) then we apply operation 
of prediction and filtering. 

EKF for nonlinear processes 

Prediction Step 

X(k+l)/(k)=X(k)/(k)+£ t +1)4t f(x(r), u k )dx (14) 
Covariance Matrix 

P (k+ 1 )/ (k) =A(k) P (k)/(k) A T (k) + Q(k) (15) 

Correction Step 

X(/c+l)/(/c+l) =[X(k+l)/(k) +K(k+l)(Y(k)-gX(k+l)/(k))] 
(16) 

Problems with EKF 

1. Uses linearized model[6] and[7] for computing covariance matrix (uncertainty) in estimates 

2. Uses a linear estimator for obtaining updated estimates 

3. Unconstrained estimator - Clipping has been adopted as a possible approach for including constraints. 

4. Has been shown to generate poor estimates 

5. Lengthy periods of over or under estimation, demonstrated in some cases to be 2 orders of magnitude. 

B. Unscented Kalman Filter (UKF) 

In unscented kalman filter, we don't need to linearize the system. We predict the pdf by sigma point approach 
[6] and calculate their weighted mean and covariance. On the basis of their weighted mean and covariance we 
select appropriate point. If 'n' is an order of system then we select 2n+l sigma point [8]. 



2n+l sigma point selection 



X 0 =X 



W 0 = (k)/ (k+n) 



(17) 



> X,= X+V((n + 



Wj= l/2(k+n) 



•(18) 
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And 

> Xi + n=X-V(( n + /c )^)i W i+n = l/2(k+n) (19) 

2n+l sigma points are propagated as 

> Y i= g(Xi) (20) 
Mean is calculated 

> Y^oWM (21) 
Covariance is calculated 

> Pyy=&m[YiJ][YiJ] T (22) 

IV. Problem statement 
Front-wheel steered robot modified: 

States: x: x co-ordinate, 
y: y co-ordinate, 

v|/: heading angle, 

a: steering angle, 

v: velocity 

Model: 

State equations: 



x 




'V cos(\|/)" 


y 




vsin(\|/) 


¥ 




v tan(a) 


d 




0.005 


_v_ 




-0.098el"- 



Output: z = [ ] 

Initial conditions: [0, 0, 0, 0, 0], t =[0, 15] 




www.ijlret.com 



33 I Page 



Basics OfKalman Filter And Position Estimation Of Front Wheel Automatic Steered Robot Car 



For some navigation application, we need to know the precise position of vehicles when it turns, for using GPS 
along with map matching system we need appropriate updated position of car so if vehicle misses its position 
i.e. the desired trajectory then kalman filter will remove the error and take vehicle onto its desired trajectory. For 
automatic vehicle system it is very important that it must know its location so that it may operate autonomously 
and reach the required destination. 

In this problem we have to find the position of car as we have been given the state equation (Process model) 
and output measurement equation. In process model there are total five parameter. First two parameter x and y 
are for coordinates of a car i.e. position of the car. *F is heading angle which changes w.r.t steering angle a. In 
process model we can clearly see that position of car is dependent on the velocity v and the heading angle but 
in this problem our aim to predict and update the position of the car(x and y coordinate) as shown in 
measurement equation. 

If we implement this problem using extended kalman filter (EKF), we need to linearize the matrix 'A' by taking 
its jacobian, then predict the position. If we take Unscented kalman filter (UKF) then no need for linearization, 
we will predict the sigma point depend on the order of the process model. In our problem the order of the system 
is 5 then UKF will predict 6 point [8]and out of them it will select 1 point on the basis of their weighted mean 
and covariance and then filtering operation. 

Initially we will consider variance matrix 'w' as null matrix therefore our initial covariance matrix will also be 
null matrix 



/0 ■•• 0 
Q= : ! 

Vo ■•• o 

Order of matrix depend upon the parameter of process model. 
EkF Implementation 

Because of the nonlinear case that is encountered here we also have used an EKF that linearizes the current 
mean and covariance. The EKF has been applied to the same robot model as the one used for the KF case. The 
EKF algorithm is described in the following: 

A. The prediction step: 

By using procees model we will predict the next step. 



X 




v cos(v|/)" 


y 




i;sin(\|/) 






v tan(a) 


a 




0.005 


-V. 




-t 

L 0.098e!"J 



First linearize it by taking its jacobian 



0 0 — vsin\\i 0 0 

J= 0 0 vcosMf 0 0 

0 0 0 vlogsina tana 

0 0 0 0 0 

0 0 0 0 0 

Where J is the jacobian of 'F'. 

From output equation we have been given C=[l 1 0 0 0] which is Clearly indicating that we have to predict and 
measure the coordinate of the car i.e the position of the car. 

B. Observation and update: 

In this step we need to compute the Kalman gain 
Refer equation (1 1),(12) and(13) 
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Result 



Results for EKF 




Fig 2:EKF Result for time period t=0.5 
On x-axis t in second , 



Results for EKF 




Fig 3:EkfF Result fortime period t=0.7 
On x-axis t in second 
Y-axis x estimated in red and x true in blue 




On x-axis t in second 
Y-axis x estimated in red and x true in blue 



Conclusion 

We have presented the basic of kalman filter and successfully implemented the kalman filter on front wheel 
automatic steered robot and got the results in scilab, as it is clearly showing on y-axis that x_estimated in (blue 
line) updated by kalman filter and bringing it back to the x_true in (red line) by compensating the posterior 
covariance. 
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By making use of klaman filter together with GPS and map matching technique we can use it in automatic 
vehicles for trajectory tracking. 
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